#! /usr/bin/env python
#coding=utf-8
import cv2
import rospy
import numpy as np
from megatron.msg import wr

rospy.init_node('post1')
print('\nPost: Post is watching.\n')
pub = rospy.Publisher("warn", wr, queue_size=10)
rec=cv2.face.LBPHFaceRecognizer_create()#modelreader
rec.read('/home/xbot/catkin_ws/src/megatron/src/include/trainer.yml')#model
#Arguments
f_det=cv2.CascadeClassifier('/home/xbot/catkin_ws/src/megatron/src/include/haarcascade_frontalface_alt.xml')#facedetector

Ypre=0
conf=0

cam=cv2.VideoCapture(0)
minw=20
minh=20
count=0

#input
#Test
while not rospy.is_shutdown():
    success, img=cam.read()
    gimg=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) #Colorgogrey#4
    f=f_det.detectMultiScale(gimg,1.2,5) #Catchface#5
    for (x,y,w,h) in f: #predicteach
        Ypre, conf=rec.predict(gimg[x:x+w,y:y+h])
        if Ypre==1 and conf > 50:
            pub.publish(wr(1))
            count=2999
            print('\nPost: Is that Optimus? Warning!\n')
    cv2.imshow('camera', gimg)
    cv2.waitKey(100)
    count+=1
    if count>=3000:
        break
#Release
cam.release()
cv2.destroyAllWindows()

